/* trackuino copyright (C) 2010  EA5HAV Javi
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */

#include "config.h"
#include "pin.h"
#include "gps.h"
#if (ARDUINO + 1) >= 100
#  include <Arduino.h>
#else
#  include <WProgram.h>
#endif
#include <stdlib.h>
#include <string.h>

// Module declarations
static void parse_sentence_type(const char * token);
static void parse_time(const char *token);
static void parse_status(const char *token);
static void parse_lat(const char *token);
static void parse_lat_hemi(const char *token);
static void parse_lon(const char *token);
static void parse_lon_hemi(const char *token);
static void parse_speed(const char *token);
static void parse_course(const char *token);
static void parse_altitude(const char *token);
static void parse_num_sats(const char *token);

// Module types
typedef void (*t_nmea_parser)(const char *token);

enum t_sentence_type {
  SENTENCE_UNK,
  SENTENCE_GGA,
  SENTENCE_RMC
};


// Module constants
static const t_nmea_parser unk_parsers[] = {
  parse_sentence_type,    // $GPxxx
};

static const t_nmea_parser gga_parsers[] = {
  NULL,             // $GPGGA
  parse_time,       // Time
  NULL,             // Latitude
  NULL,             // N/S
  NULL,             // Longitude
  NULL,             // E/W
  NULL,             // Fix quality 
  parse_num_sats,   // Number of satellites
  NULL,             // Horizontal dilution of position
  parse_altitude,   // Altitude
  NULL,             // "M" (mean sea level)
  NULL,             // Height of GEOID (MSL) above WGS84 ellipsoid
  NULL,             // "M" (mean sea level)
  NULL,             // Time in seconds since the last DGPS update
  NULL              // DGPS station ID number
};

static const t_nmea_parser rmc_parsers[] = {
  NULL,             // $GPRMC
  parse_time,       // Time
  parse_status,     // A=active, V=void
  parse_lat,        // Latitude,
  parse_lat_hemi,   // N/S
  parse_lon,        // Longitude
  parse_lon_hemi,   // E/W
  parse_speed,      // Speed over ground in knots
  parse_course,     // Track angle in degrees (true)
  NULL,             // Date (DDMMYY)
  NULL,             // Magnetic variation
  NULL              // E/W
};

static const int NUM_OF_UNK_PARSERS = (sizeof(unk_parsers) / sizeof(t_nmea_parser));
static const int NUM_OF_GGA_PARSERS = (sizeof(gga_parsers) / sizeof(t_nmea_parser));
static const int NUM_OF_RMC_PARSERS = (sizeof(rmc_parsers) / sizeof(t_nmea_parser));

// Module variables
static t_sentence_type sentence_type = SENTENCE_UNK;
static bool at_checksum = false;
static unsigned char our_checksum = '$';
static unsigned char their_checksum = 0;
static char token[16];
static int num_tokens = 0;
static unsigned int offset = 0;
static bool active = false;
static char gga_time[7] = "", rmc_time[7] = "";
static char new_time[7];
static uint32_t new_seconds;
static float new_lat;
static float new_lon;
static char new_aprs_lat[9];
static char new_aprs_lon[10];
static float new_course;
static float new_speed;
static float new_altitude;
static byte  new_num_sats;

// Public (extern) variables, readable from other modules
char gps_time[7];       // HHMMSS
uint32_t gps_seconds = 0;   // seconds after midnight
float gps_lat = 0;
float gps_lon = 0;
char gps_aprs_lat[9];
char gps_aprs_lon[10];
float gps_course = 0;
float gps_speed = 0;
float gps_altitude = 0;
byte  gps_num_sats = 0;
bool  gps_low_power_mode = false;


// Ublox definitions

// Cyclic (Power Save)
const uint8_t setPSM[] PROGMEM = { 
  0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 
  0x92 };


// Max Performance
const uint8_t setMax[] PROGMEM = { 
  0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x00, 0x21, 
  0x91 }; 

// Cold reset GPS
const uint8_t set_reset[] PROGMEM = { 
  0xB5, 0x62, 0x06, 0x04, 0x04, 0x00, 0xFF, 0x87, 0x00, 
  0x00, 0x94, 0xF5 };

// Set to Flight (Airborne) mode
const uint8_t setNav[] PROGMEM = {
  0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 
  0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 
  0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC };

// Disable GGL Sentences
const uint8_t setGLL[] PROGMEM = { 
  0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x01, 0x01, 0x2B };

// Disable GSA Sentences
const uint8_t setGSA[] PROGMEM = { 
  0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x01, 0x02, 0x32 };

// Disable GSV Sentences
const uint8_t setGSV[] PROGMEM = { 
  0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x01, 0x03, 0x39 };

// Disable VTG Sentences
const uint8_t setVTG[] PROGMEM = { 
  0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x04, 0x46 };


// Module functions
unsigned char from_hex(char a) 
{
  if (a >= 'A' && a <= 'F')
    return a - 'A' + 10;
  else if (a >= 'a' && a <= 'f')
    return a - 'a' + 10;
  else if (a >= '0' && a <= '9')
    return a - '0';
  else
    return 0;
}

void parse_sentence_type(const char *token)
{
  if (strcmp(token, "$GPGGA") == 0) {
    sentence_type = SENTENCE_GGA;
  } else if (strcmp(token, "$GNGGA") == 0) {
    sentence_type = SENTENCE_GGA;
  } else if (strcmp(token, "$GPRMC") == 0) {
    sentence_type = SENTENCE_RMC;
  } else if (strcmp(token, "$GNRMC") == 0) {
    sentence_type = SENTENCE_RMC;
  } else {
    sentence_type = SENTENCE_UNK;
  }
}

void parse_time(const char *token)
{
  // Time can have decimals (fractions of a second), but we only take HHMMSS
  strncpy(new_time, token, 6);
  // Terminate string
  new_time[6] = '\0';
  
  new_seconds = 
    ((new_time[0] - '0') * 10 + (new_time[1] - '0')) * 60 * 60UL +
    ((new_time[2] - '0') * 10 + (new_time[3] - '0')) * 60 +
    ((new_time[4] - '0') * 10 + (new_time[5] - '0'));
}

void parse_status(const char *token)
{
  // "A" = active, "V" = void. We shoud disregard void sentences
  if (strcmp(token, "A") == 0)
    active = true;
  else
    active = false;
}

void parse_lat(const char *token)
{
  // Parses latitude in the format "DD" + "MM" (+ ".M{...}M")
  char degs[3];
  if (strlen(token) >= 4) {
    degs[0] = token[0];
    degs[1] = token[1];
    degs[2] = '\0';
    new_lat = atof(degs) + atof(token + 2) / 60;
  }
  // APRS-ready latitude
  strncpy(new_aprs_lat, token, 7);
  new_aprs_lat[7] = '\0';
}

void parse_lat_hemi(const char *token)
{
  if (token[0] == 'S')
    new_lat = -new_lat;
  new_aprs_lat[7] = token[0];
  new_aprs_lon[8] = '\0';
}

void parse_lon(const char *token)
{
  // Longitude is in the format "DDD" + "MM" (+ ".M{...}M")
  char degs[4];
  if (strlen(token) >= 5) {
    degs[0] = token[0];
    degs[1] = token[1];
    degs[2] = token[2];
    degs[3] = '\0';
    new_lon = atof(degs) + atof(token + 3) / 60;
  }
  // APRS-ready longitude
  strncpy(new_aprs_lon, token, 8);
  new_aprs_lon[8] = '\0';
}

void parse_lon_hemi(const char *token)
{
  if (token[0] == 'W')
    new_lon = -new_lon;
  new_aprs_lon[8] = token[0];
  new_aprs_lon[9] = '\0';
}

void parse_speed(const char *token)
{
  new_speed = atof(token);
}

void parse_course(const char *token)
{
  new_course = atof(token);
}

void parse_altitude(const char *token)
{
  new_altitude = atof(token);
}

void parse_num_sats(const char *token) {
  // Thanks to Darkside for this code:
  // https://code.google.com/p/project-horus/source/browse/trunk/trackuino/gps.cpp
  //
  /* convert our token to an integer */
  int temp = atoi(token);    

  /* range check it's between 0-32 satellites */
  if(temp < 0)     
    temp = 0;        

  if(temp > 32)     temp = 32;    /* store value away */

  new_num_sats = temp;
}

//
// The Ublox code was taken from http://ava.upuaut.net/store (check the wiki)
//

// Calculate expected UBX ACK packet and parse UBX response from GPS
boolean getUBX_ACK(const uint8_t *MSG) {
  uint8_t b;
  uint8_t ackByteID = 0;
  uint8_t ackPacket[10];
  unsigned long startTime = millis();

  //#ifdef DEBUG_GPS
  //  debugSerial.print(" * Reading ACK response: ");
  //#endif

  // Construct the expected ACK packet    
  ackPacket[0] = 0xB5;		// header
  ackPacket[1] = 0x62;		// header
  ackPacket[2] = 0x05;		// class
  ackPacket[3] = 0x01;		// id
  ackPacket[4] = 0x02;		// length
  ackPacket[5] = 0x00;
  ackPacket[6] = pgm_read_byte(&MSG[2]);	// ACK class
  ackPacket[7] = pgm_read_byte(&MSG[3]);	// ACK id
  ackPacket[8] = 0;			// CK_A
  ackPacket[9] = 0;			// CK_B

  // Calculate the checksums
  for (uint8_t i=2; i<8; i++) {
    ackPacket[8] = ackPacket[8] + ackPacket[i];
    ackPacket[9] = ackPacket[9] + ackPacket[8];
  }

  while (1) {
    // Test for success
    if (ackByteID > 9) {
      // All packets in order!

      //#ifdef DEBUG_GPS
      //      debugSerial.println(" (SUCCESS!)");
      //#endif
      return true;
    }

    // Timeout if no valid response in 3 seconds
    if (millis() - startTime > 3000) { 
      //#ifdef DEBUG_GPS
      //      debugSerial.println(" (FAILED!)");
      //#endif
      return false;
    }

    // Make sure data is available to read
    if (GPS_SERIAL.available()) {
      b = GPS_SERIAL.read();

      // Check that bytes arrive in sequence as per expected ACK packet
      if (b == ackPacket[ackByteID]) { 
        ackByteID++;

        //#ifdef DEBUG_GPS
        //        debugSerial.print(b, HEX);
        //#endif
      } 
      else {
        ackByteID = 0;	// Reset and look again, invalid order
      }
    }
  }
}

// Send a byte array of UBX protocol to the GPS
void sendUBX(const uint8_t *MSG, uint8_t len) {
  uint8_t temp;

  for(int i=0; i<len; i++) {
    temp = pgm_read_byte(&MSG[i]);

    GPS_SERIAL.write(temp);

    //#ifdef DEBUG_GPS
    //    debugSerial.print(MSG[i], HEX);
    //#endif
  }
  GPS_SERIAL.println();
}

//
// The following routines came from M0UPU (http://ava.upuaut.net/)
//

void setGPS_PowerSaveMode() {
  // Power Save Mode 
  sendUBX(setPSM, sizeof(setPSM)/sizeof(uint8_t));
}

void setGps_MaxPerformanceMode() {
  //Set GPS for Max Performance Mode
  sendUBX(setMax, sizeof(setMax)/sizeof(uint8_t));
}

void resetGPS() {
  // Cold Boot GPS
  sendUBX(set_reset, sizeof(set_reset)/sizeof(uint8_t));
}


//
// Exported functions
//
void gps_setup() {
  int gps_success = 0;
  
  strcpy(gps_time, "000000");
  strcpy(gps_aprs_lat, "0000.00N");
  strcpy(gps_aprs_lon, "00000.00E");
  
#ifdef GPS_USING_UBLOX  
  // Setup for the uBLOX MAX-6/7/8
  
  // Check to see if we need to power up the GPS
#ifdef GPS_POWER_PIN
  pinMode(GPS_POWER_PIN,   OUTPUT);
  pin_write(GPS_POWER_PIN, LOW);
#endif

#ifdef GPS_POWER_SLEEP_TIME  
  // Add a bit of a delay here to allow the GPS a chance to wake up
  // (even if we aren't powering it via a digital pin)
  delay(GPS_POWER_SLEEP_TIME);
#endif  

#ifdef GPS_LED_PIN
  // LED for GPS Status
  pinMode(GPS_LED_PIN,      OUTPUT);
  pin_write(GPS_LED_PIN,    LOW);
#endif

  // Set MAX-6 to flight mode
  int setup_attempts_remaining = 120;
  while (!gps_success && (setup_attempts_remaining--) >= 0) {
    sendUBX(setNav, sizeof(setNav)/sizeof(uint8_t)); 
    gps_success = getUBX_ACK(setNav);
	if (!gps_success) {
	  // Should find a better way to indicate a problem?
#ifdef GPS_LED_PIN
      pin_write(GPS_LED_PIN, HIGH);
#endif	  
      delay(500);
	  }
	}
  
#ifdef GPS_LED_PIN
  pin_write(GPS_LED_PIN, LOW);
#endif  

  // Finally turn off any NMEA sentences we don't need (in this case it's 
  // everything except GGA and RMC)
  sendUBX(setGLL, sizeof(setGLL)/sizeof(uint8_t));   // Disable GGL
  sendUBX(setGSA, sizeof(setGSA)/sizeof(uint8_t));   // Disable GSA
  sendUBX(setGSV, sizeof(setGSV)/sizeof(uint8_t));   // Disable GSV
  sendUBX(setVTG, sizeof(setVTG)/sizeof(uint8_t));   // Disable VTG
#endif
}

bool gps_decode(char c)
{
  int ret = false;

  switch(c) {
    case '\r':
    case '\n':
      // End of sentence

      if (num_tokens && our_checksum == their_checksum) {
#ifdef DEBUG_GPS
        Serial.print(" (OK!) ");
        Serial.print(millis());
#endif
        // Return a valid position only when we've got two rmc and gga
        // messages with the same timestamp.
        switch (sentence_type) {
          case SENTENCE_UNK:
            break;    // Keeps gcc happy
          case SENTENCE_GGA:
            strcpy(gga_time, new_time);
            break;
          case SENTENCE_RMC:
            strcpy(rmc_time, new_time);
            break;
        }

        // Valid position scenario:
        //
        // 1. The timestamps of the two previous GGA/RMC sentences must match.
        //
        // 2. We just processed a known (GGA/RMC) sentence. Suppose the
        //    contrary: after starting up this module, gga_time and rmc_time
        //    are both equal (they're both initialized to ""), so (1) holds
        //    and we wrongly report a valid position.
        //
        // 3. The GPS has a valid fix. For some reason, the Venus 634FLPX
        //    reports 24 deg N, 121 deg E (the middle of Taiwan) until a valid
        //    fix is acquired:
        //
        //    $GPGGA,120003.000,2400.0000,N,12100.0000,E,0,00,0.0,0.0,M,0.0,M,,0000*69 (OK!)
        //    $GPGSA,A,1,,,,,,,,,,,,,0.0,0.0,0.0*30 (OK!)
        //    $GPRMC,120003.000,V,2400.0000,N,12100.0000,E,000.0,000.0,280606,,,N*78 (OK!)
        //    $GPVTG,000.0,T,,M,000.0,N,000.0,K,N*02 (OK!)

        if (sentence_type != SENTENCE_UNK &&      // Known sentence?
            strcmp(gga_time, rmc_time) == 0 &&    // RMC/GGA times match?
            active) {                             // Valid fix?
          // Atomically merge data from the two sentences
          strcpy(gps_time, new_time);
          gps_seconds = new_seconds;
          gps_lat = new_lat;
          gps_lon = new_lon;
          strcpy(gps_aprs_lat, new_aprs_lat);
          strcpy(gps_aprs_lon, new_aprs_lon);
          gps_course = new_course;
          gps_speed = new_speed;
          gps_altitude = new_altitude;
          gps_num_sats = new_num_sats;
		  
#ifdef GPS_USING_UBLOX
          if (gps_num_sats >= MIN_NO_CYCLIC_SATS) {
            if (!gps_low_power_mode) {
              setGPS_PowerSaveMode();
              gps_low_power_mode = true;
			}
		  } 
		  else {
            // Check to see if we WERE in low-power mode and either lost the
            // fix or were partially obscured
            if (gps_low_power_mode) {
              // Yeah.  Switch back to max performance until we get more
              // satellites
              gps_low_power_mode = false;
              setGps_MaxPerformanceMode();
			}
		  }
#endif		  

          ret = true;
        }
      }
#ifdef DEBUG_GPS
      if (num_tokens)
        Serial.println();
#endif
      at_checksum = false;        // CR/LF signals the end of the checksum
      our_checksum = '$';         // Reset checksums
      their_checksum = 0;
      offset = 0;                 // Prepare for the next incoming sentence
      num_tokens = 0;
      sentence_type = SENTENCE_UNK;
      break;
    
    case '*':
      // Handle as ',', but prepares to receive checksum (ie. do not break)
      at_checksum = true;
      our_checksum ^= c;

    case ',':
      // Process token
      token[offset] = '\0';
      our_checksum ^= c;  // Checksum the ',', undo the '*'

      // Parse token
      switch (sentence_type) {
        case SENTENCE_UNK:
          if (num_tokens < NUM_OF_UNK_PARSERS && unk_parsers[num_tokens])
            unk_parsers[num_tokens](token);
          break;
        case SENTENCE_GGA:
          if (num_tokens < NUM_OF_GGA_PARSERS && gga_parsers[num_tokens])
            gga_parsers[num_tokens](token);
          break;
        case SENTENCE_RMC:
          if (num_tokens < NUM_OF_RMC_PARSERS && rmc_parsers[num_tokens])
            rmc_parsers[num_tokens](token);
          break;
      }

      // Prepare for next token
      num_tokens++;
      offset = 0;
#ifdef DEBUG_GPS
      Serial.print(c);
#endif
      break;

    default:
      // Any other character
      if (at_checksum) {
        // Checksum value
        their_checksum = their_checksum * 16 + from_hex(c);
      } else {
        // Regular NMEA data
        if (offset < 15) {  // Avoid buffer overrun (tokens can't be > 15 chars)
          token[offset] = c;
          offset++;
          our_checksum ^= c;
        }
      }
#ifdef DEBUG_GPS
      Serial.print(c);
#endif
  }
  return ret;
}

